import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.robotics.subsumption.Behavior;

/**
 * Permet de suivre la piste, gris foncé.
 * gris clair, trop à droite, intermédiaire, trop à gauche blanc : intersection ou changement de direction, noir : changement de cellule
 * @author Nicolas Lefebvre
 * @author Emilie Colin
 */
public class SuivreLigne implements Behavior {

    /**
     *  Prise du contrôle du comportement
     * @return true quand le robot a à suivre la ligne
     */
    public boolean takeControl(){
        try{
            return Robot.estSurLaLigne();
        }catch(Exception ex){
              Robot.justSay("Erreur lecture gris léger");
              return false;
        }
    }
    
    /**
     * Méthode immédiatement appelée après action
     * @see action()
     */
    public void suppress() {
        try{
            //Robot.Pilot.steer(0);
            //Robot.Pilot.forward();
            Robot.CHANGEMENT_CELLULE=true;
        }catch(Exception ex){
              Robot.justSay("Erreur suppress suivre ligne");
        }
    }

    public void action() {
        try{
            
            //LCD.clear();
            LCD.drawString("Suivre ligne      !", 0, 0);
            LCD.refresh();
            Robot.Pilot.forward();
            int pas = 3;
            float pas_steer=4;
            int i=0;
            int lightValue;
            int newLightValue;
            int turnRate=100;
            int power = Robot.MAX_POWER;
            int new_power=Robot.MAX_POWER*100/125;
            LCD.drawString("power : "+power, 0, 4);
            LCD.drawString("new power : "+new_power, 0, 5);
            while (!Robot.estSurDuNoir()){
                if(Robot.UltrasonicSensor.getDistance() <= Robot.SEUIL_OBSTACLE){
                    Robot.OBSTACLE_DETECTE=true;
                }
                LCD.drawString("valeur"+Robot.readLightValue(), 0, 1);
                LCD.refresh();
                lightValue=Robot.readLightValue();
                
                if(Robot.GRIS_FONCE.contient(lightValue)){
                    LCD.clear();
                    LCD.drawString("on s'ecarte ! "+Robot.readLightValue(), 0, 1);
                    LCD.refresh();
            
                    if(pas>0){
                        Robot.MoteurDroit.stop();
                        Robot.MoteurGauche.forward();
                    } else{
                        Robot.MoteurDroit.forward();
                        Robot.MoteurGauche.stop();
                     }

                    //Robot.Pilot.rotate(pas);
                    //Si après rotation on s'est éloigné : pas normal, on s'est planté de sens
                    if(Robot.readLightValue()<lightValue){
                        //On s'est éloigné => on tournera dans l'autre sens à la prochaine itération
                        //pas_steer = pas_steer;
                        turnRate = 0-turnRate;

                    if(pas<0){
                        Robot.MoteurDroit.stop();
                        Robot.MoteurGauche.forward();
                    } else{
                        Robot.MoteurDroit.forward();
                        Robot.MoteurGauche.stop();
                    }
					
                    pas=0-pas;

                    } 
                } else {
                    Robot.Pilot.forward();
                } 

				LCD.drawString("iteration "+i, 0, 2);
				LCD.refresh();
				i++;
				
            }
            //On est arrivé en bout de case => on traverse la ligne en avançant de 4cm
            Robot.Pilot.travel(4f);
            //On vient de changer de case, on met la position du robot à jour
            Robot.mettreAJourPosition();
            
            LCD.drawString("Sortie "+i, 0, 3);
            LCD.refresh();
            
        }catch(Exception ex){
              Robot.justSay("Erreur action suivre ligne");
        }
    }  
}
